perm filename CAMERA.SAI[SYS,HE]8 blob sn#050973 filedate 1973-06-28 generic text, type T, neo UTF8
COMMENT ⊗   VALID 00015 PAGES 
RECORD PAGE   DESCRIPTION
 00001 00001
 00003 00002	BEGIN "GILCAM" 
 00007 00003	SIMPLE PROCEDURE DATXFR  ⊃   READS DATA IN GILL'S FORMAT
 00009 00004	SIMPLE PROCEDURE SERVO
 00012 00005	SIMPLE REAL PROCEDURE LNS(REAL X INTEGER RLENS)
 00013 00006	PROCEDURE PANTIL_CAM(INTEGER CLENSREAL PPOT,TPOT,FPOT,ZPOT
 00017 00007	SIMPLE MESSAGE PROCEDURE CAM_UPDATE(INTEGER CAMNO)
 00020 00008	IF IND>0 THEN
 00023 00009	
 00026 00010	SIMPLE PROCEDURE HAT(INTEGER CAMNO)
 00027 00011	SIMPLE MESSAGE PROCEDURE CHNG_LENS(INTEGER RLENS)
 00032 00012	SIMPLE MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS,CAMNO REAL XTC,YTC,ZTC)
 00036 00013	
 00040 00014	SIMPLE PROCEDURE TESTIT
 00044 00015	⊃ HERE BEGINS EXECUTION 
 00045 ENDMK
⊗;
BEGIN "GILCAM" 
REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
REQUIRE "HELIB[1,3]" LIBRARY;
require "drvpgm[sys,he]" load_module;
REQUIRE -1 NEW_ITEMS;

DEFINE ⊃="COMMENT";

⊃ THIS IS THE CAMERA MINDER .  IT HOUSES THE FOLLOWING MESSAGE PROCEDURES:
  CAM_INIT-INITIALIZES THE CAMERA MODEL FROM ONE OF IRWIN'S RELAXED MODELS.
  CAM_UPDATEATE-READS THE POTS AND CREATES A NEW  CAMERA TRANSFORM MATRIX.
  CHNG_LENS(LENS), MOVE_CAM(PAN,TILT), CHNG_FOCUS(RANGE)-DO THE OBVIOUS.
  CAM_CENTER(LENS,X,Y,Z)-CENTER THE CAMERA ON TABLE COORDS X,Y,Z.
  CAM_PRED(LENS,X,Y,Z)-COMPUTE THE MODEL FOR THE CAMERA ON TABLE COORDS X,Y,Z,
		       AND STORE IT INTO CAMERA_PREDI.

  THE CAMERA TRANSFROM MATRIX IS A 10X3 MATRIX:
	1:3 X 1:3   THE COLINEATION MATRIX.
	 4  X 1:3   THE LENS CENTER.
	 5  X 1:3   THE CENTER OF THE FIELD OF VIEW
	6:8 X 1:3   THE INVERSE COLINEATION MATRIX.
	 9  X 1:3   CAMPAN, CAMTIL, CAMRANG
	 10 X 1:2   CAMERA NO., CAMLENS

   A NEW ITEM IS GENERATED, PUT IN CURCAM, AND THIS MATRIX BECOMES
   ITS DATUM.
   ERRORS: 1-SERVO PROBLEMS(SERVO). 
	   2-LENS OUT OF BOUNDS(CHNG_LENS,CAM_CENTER).
	   3-PAN OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
	   4-TILT OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
	   5-FOCUS OUT OF BOUNDS(CHNG_FOCUS,CAM_CENTER). 
	   6-LENS CHANGER GOOFED(CHNG_LENS).
	   7-LOOKUP FAILED(DATXFR). 
	   8-NOT ENOUGH READINGS(CAM_UPDATE).
           9-POTS TOO NOISY(CAM_UPDATE). 
	  10-AD NOT AVAILABLE(CAM_UPDATE).
;

REAL ARRAY MCOL,MICOL [1:3,1:3];  ⊃ WHERE I KEEP CURRENT CAMERA XFORM;
REAL ARRAY LCEN[1:3];		  ⊃ AND LENS CENTER ;
REAL ZPOT0,ZPOTD,REF,FOC_LGTH;
EXTERNAL REAL SREF,CREF;

REAL ARRAY PPOT0,PPOTD,TPOT0,TPOTD,FPOT0,FPOTD,MART,SWING,GROREF,
           FOC,FOCLEN0,FOCLENG[1:5],DP,P0[1:5,1:3],PP[1:5,1:2];
INTEGER MESS,BLOCK,DUMMY,EOF;

DEFINE XDATA="3",STX="1 STEP 1 UNTIL",CRLF="&'15&'12",
 	YES="INCHWL=""Y""",KY="684.5",C1="2.1249",C2="-.0006";

preload_with 2000.,4096.;
real array plimit1[1:2];

preload_with -2000.,-4096.;
real array plimit2[1:2];

preload_with -340.,4096.;
real array tlimit1[1:2];

preload_with -1870.,-4096.;
real array tlimit2[1:2];

preload_with 1900.,4096.;
real array flimit1[1:2];

preload_with 0.0,-4096.;
real array flimit2[1:2];


SIMPLE PROCEDURE DATXFR;  ⊃   READS DATA IN GILL'S FORMAT;
  BEGIN BOOLEAN FLAG;
        INTEGER DUMMY;
	REQUIRE "⊂⊃⊂⊃" DELIMITERS;
	DEFINE DATASET= ⊂(IF CAMNUM=1 THEN "DATA[SYS,HE]" 
					ELSE "DATA2[SYS,HE]")⊃;
	REQUIRE UNSTACK_DELIMITERS;
	DEFINE XFR(X)="ARRYIN(XDATA,X,1)";
	OPEN(XDATA,"DSK",12,3,0,0,0,0);
	LOOKUP(XDATA,DATASET,FLAG);
	IF FLAG THEN BEGIN OUTSTR("DATXFR-FAILED: LOOKUP FAILED FOR "&
				   DATASET CRLF);
 			CAMFLG←7; RETURN; END;
	IF TYP_CAM THEN OUTSTR("DATXFR: RETRIEVING "&DATASET&CVS(BLOCK)CRLF);
	USETI(XDATA,1); DUMMY←WORDIN(XDATA);
	USETI(XDATA,BLOCK);
	IF CAMNUM=2 THEN BEGIN
			BLOCK←5;
			XFR(ZPOT0);
			XFR(ZPOTD);
			END;
    XFR(PPOT0[BLOCK]); XFR(PPOTD[BLOCK]); XFR(TPOT0[BLOCK]); XFR(TPOTD[BLOCK]);
    XFR(FPOT0[BLOCK]); XFR(FPOTD[BLOCK]);
    XFR(MART[BLOCK]);  XFR(SWING[BLOCK]);
    XFR("PP[BLOCK,1]");XFR("PP[BLOCK,2]");XFR("P0[BLOCK,1]");XFR("P0[BLOCK,2]");
    XFR("P0[BLOCK,3]");XFR("DP[BLOCK,1]");XFR("DP[BLOCK,2]");XFR("DP[BLOCK,3]");
    XFR(FOC[BLOCK]); XFR(FOCLEN0[BLOCK]); XFR(FOCLENG[BLOCK]);	XFR(GROREF[BLOCK]);
	RELEASE(XDATA);
  END "DATXFR";

SIMPLE PROCEDURE SERVO;
  BEGIN	EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
	EXTERNAL PROCEDURE SSERVO;
	EXTERNAL REAL L1,L2,L3,L4,L5,L6,L7,L8,e1,e2,e3;
	EXTERNAL REAL P1,P2,P3,P4,P5,P6,P7,P8;
	EXTERNAL INTEGER STATUS,TSERVO;
	EXTERNAL INTEGER STATE;
        EXTERNAL PROCEDURE SPWOFF;

 IF CAMNUM=1 THEN BEGIN
    SPWON(1,TSERVO);
    WHILE ¬(STATUS LAND 1) DO;
    IF (STATUS≥'100)∧(STATUS<'100000) THEN BEGIN
       OUTSTR("SERVO-FAILED: STATUS="&CVOS(STATUS)CRLF);
       CAMFLG←1;        END;
    SPWOFF; CALL(1,"SLEEP"); 
    IF DEB_CAM THEN BEGIN
    OUTSTR("SERVO: E1="&CVF(E1)&"  E2="&CVF(E2)&"  E3="&CVF(E3)CRLF);
    OUTSTR("       L1="&CVF(L1)&"  L2="&CVF(L2)&"  L3="&CVF(L3)CRLF);
    OUTSTR("       P1="&CVF(P1)&"  P2="&CVF(P2)&"  P3="&CVF(P3)CRLF); END;
    END;

 IF CAMNUM=2 THEN BEGIN
	SSERVO;
	WHILE STATE=0 DO;
	IF (STATE≥'100)∧(STATE<'100000) THEN  BEGIN
	    OUTSTR("SIERRA SERVO FAILED: STATUS="&CVOS(STATE)CRLF);
        CAMFLG←1;
        END;
	IF DEB_CAM THEN BEGIN
          OUTSTR("SERVO: L4="&CVF(L4)&" L5="&CVF(L5)&" L6="&CVF(L6)&" L7="&CVF(L7)&
                       " L8="&CVF(L8)CRLF);
          OUTSTR("       P4="&CVF(P4)&" P5="&CVF(P5)&" P6="&CVF(P6)&" P7="&CVF(P7)&
                       " P8="&CVF(P8)CRLF);
          END;
        END;
  END "SERVO";

SIMPLE REAL PROCEDURE LNS(REAL X; INTEGER RLENS);
      RETURN (X*FOC[RLENS]/(X-FOC[RLENS]));


PROCEDURE PANTIL_CAM(INTEGER CLENS;REAL PPOT,TPOT,FPOT,ZPOT;
  		      REAL ARRAY COL,ICOL,CENTER);
 ⊃ Compute the transformation matrices for the camera model
   given by LENS and the last pots readings;
  BEGIN INTEGER I,J;   
	REAL   ACC,FMX,FMY,PAN,TILT;
        REAL ARRAY RP,RT,RPT,RS,R[1:3,1:3],C[1:3];
	FORTRAN REAL PROCEDURE SIN;
	FORTRAN REAL PROCEDURE COS;
	REQUIRE "SOBMAT[SYS,HE]" LOAD_MODULE;
	EXTERNAL PROCEDURE INVRT(REAL ARRAY A,AI);

	PAN←PPOTD[CLENS]*PPOT+PPOT0[CLENS];
        TILT←TPOTD[CLENS]*TPOT+TPOT0[CLENS];
	FMY←FPOTD[CLENS]*FPOT+FPOT0[CLENS];
		IF CAMNUM=2 THEN FMY←FMY+ZPOTD/(ZPOT-ZPOT0);
                FMX←FMY*MART[CLENS];

	RP[2,3]←-1;    RPT[1,1]←RP[1,1]←RP[3,2]←-SIN(PAN);
        RP[3,1]←-(RPT[1,2]←RP[1,2]←COS(PAN));
	RT[1,1]←1;     RPT[2,3]←-(RT[2,2]←RT[3,3]←COS(TILT));
        R[3,3]←RPT[3,3]←RT[2,3]←-(RT[3,2]←SIN(TILT));
	RPT[2,1]←RT[2,3]*RP[3,1];     RPT[2,2]←RT[2,3]*RP[3,2]; 
	R[3,1]←RPT[3,1]←RT[3,3]*RP[3,1];     R[3,2]←RPT[3,2]←RT[3,3]*RP[3,2];
	RS[3,3]←1;  RS[1,1]←RS[2,2]←COS(SWING[CLENS]);
	RS[2,1]←-(RS[1,2]←SIN(SWING[CLENS]));
	R[1,1]←RS[1,1]*RPT[1,1]+RS[1,2]*RPT[2,1];
	R[1,2]←RS[1,1]*RPT[1,2]+RS[1,2]*RPT[2,2];   R[1,3]←RS[1,2]*RPT[2,3];
	R[2,1]←RS[2,1]*RPT[1,1]+RS[2,2]*RPT[2,1];
	R[2,2]←RS[2,1]*RPT[1,2]+RS[2,2]*RPT[2,2];    R[2,3]←RS[2,2]*RPT[2,3];

	C[1]←P0[CLENS,1]+R[1,1]*DP[CLENS,1]+R[2,1]*DP[CLENS,2]+R[3,1]*DP[CLENS,3];
	C[2]←P0[CLENS,2]+R[1,2]*DP[CLENS,1]+R[2,2]*DP[CLENS,2]+R[3,2]*DP[CLENS,3];
	C[3]←P0[CLENS,3]+R[1,3]*DP[CLENS,1]+R[2,3]*DP[CLENS,2]+R[3,3]*DP[CLENS,3];

	⊃ -FMX/333 and PP[2]-PP[1]/333 are corrections for
	  the non-orthogonality of the TV scan axes;
	FOR I←1 STEP 1 UNTIL 3 DO BEGIN 
      	    COL[I,1]←R[I,1];   COL[I,2]←R[I,2];   ACC←0;
	    FOR J←1 STEP 1 UNTIL 3 DO	ACC←ACC-R[I,J]*C[J];
	    COL[I,3]←ACC    END;
	FOR J←1 STEP 1 UNTIL 3 DO BEGIN 
	    COL[2,J]←-FMX/333*COL[1,J]+FMY*COL[2,J]
		     +(PP[CLENS,2]-PP[CLENS,1]/333)*COL[3,J];
	    COL[1,J]←FMX*COL[1,J]+PP[CLENS,1]*COL[3,J]; END;
    INVRT(COL,ICOL); ARRTRAN(CENTER,C);
  END "PANTIL_CAM";
SIMPLE MESSAGE PROCEDURE CAM_UPDATE(INTEGER CAMNO);
  BEGIN REAL SFOC,STIL,SPAN,IND,FMAX,FMIN,TMAX,TMIN,PMAX,PMIN,SZOM,ZMAX,ZMIN,
	        DIFFOC,DIFTIL,DIFPAN,DIFZOM,SIND;
	EXTERNAL REAL L1,L2,L3,P1,P2,P3;
        EXTERNAL INTEGER STATUS,TSERVO;
	EXTERNAL REAL L4,L5,L6,L7,L8,P4,P5,P6,P7,P8;
	EXTERNAL INTEGER STATE;
        INTEGER UPDFLG;
	FORTRAN REAL PROCEDURE SQRT(REAL X);
       	EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
        EXTERNAL PROCEDURE SPWOFF;
	EXTERNAL PROCEDURE scalpot;
	LABEL ETA;

IF CAMNO=1 THEN BEGIN

 ETA:	SFOC←STIL←SPAN←0; FMAX←TMAX←PMAX←-10000; FMIN←TMIN←PMIN←10000;
	STATUS←1; SPWON(1,TSERVO);
	FOR IND←0 STEP 1 UNTIL 39 DO BEGIN
	    STATUS←4; WHILE ¬(STATUS LAND 1) DO;
	    IF (STATUS≥'100)∧(STATUS<'100000) THEN DONE;
	    SFOC←SFOC+P1; STIL←STIL+P2; SPAN←SPAN+P3;
	    IF P1>FMAX THEN FMAX←P1; IF P1<FMIN THEN FMIN←P1;
	    IF P2>TMAX  THEN TMAX←P2;  IF P2<TMIN  THEN TMIN←P2;
	    IF P3>PMAX   THEN PMAX←P3;   IF P3<PMIN   THEN PMIN←P3;
	          		     END;
	SPWOFF;
	END;

IF CAMNO=2 THEN BEGIN

	SFOC←SPAN←STIL←SZOM←0; FMAX←TMAX←PMAX←ZMAX←-10000; FMIN←TMIN←PMIN←ZMIN←10000;
	FOR IND←0 STEP 1 UNTIL 39 DO BEGIN
		scalpot;
		SPAN←SPAN+P4; SFOC←SFOC+P6;
		STIL←STIL+P5; SZOM←SZOM+P7;
		IF P4>PMAX THEN PMAX←P4; IF P4<PMIN THEN PMIN←P4;
		IF P5>TMAX THEN TMAX←P5; IF P5<TMIN THEN TMIN←P5;
		IF P6>FMAX THEN FMAX←P6; IF P6<FMIN THEN FMIN←P6;
		IF P7>ZMAX THEN ZMAX←P7; IF P7<ZMIN THEN ZMIN←P7;
		END;
	END;


IF IND>0 THEN
       BEGIN
       ref← if camno=1 then cref else GROREF[5];
       L1←FOCPOT←SFOC*REF/IND; L2←TILPOT←STIL*REF/IND; L3←PANPOT←SPAN*REF/IND;
       IF CAMNO=2 THEN L7←ZOOPOT←SZOM*REF/IND;
       IF DEB_CAM THEN BEGIN
		       OUTSTR("CAM_UPDATE: "&
		       "FP="&CVG(FOCPOT)&"  TP="&CVG(TILPOT)&"  PP="&CVG(PANPOT));
                       IF CAMNO=2 THEN OUTSTR("  ZP="&CVG(ZOOPOT)CRLF)
			ELSE OUTSTR(" "CRLF);
			END;

       IF   IND<30 THEN
            BEGIN
            OUTSTR("CAM_UPDATE: NOT ENOUGH READINGS  "&
				CVS(IND)&" "CRLF);
            UPDFLG←8;
	    END
            ELSE BEGIN
                 DIFFOC←(FMAX-FMIN)*REF; DIFTIL←(TMAX-TMIN)*REF; DIFPAN←(PMAX-PMIN)*REF;
                 IF CAMNO=2 THEN DIFZOM←(ZMAX-ZMIN)*REF;
                 IF DEB_CAM THEN BEGIN
                                 OUTSTR("CAM_UPDATE: "&
                      	             "DF="&CVS(DIFFOC)&"  DT="&CVS(DIFTIL)&"  DP="&CVS(DIFPAN));
                                 IF CAMNO=2 THEN OUTSTR("  DZ="&CVS(DIFZOM)CRLF)
						ELSE OUTSTR(" "CRLF);
                                 END;
                 
                 SIND←4*SQRT(IND);
                 IF (DIFPAN/SIND>.75)∨(DIFTIL/SIND>.75)∨(DIFFOC/SIND>1)
				∨ (CAMNO=2 ∧ DIFZOM/SIND>1)
		  THEN BEGIN
                       OUTSTR("CAM_UPDATE: POTS TOO NOISY "&CVF(DIFFOC)&"  "&
			     CVF(DIFTIL)&"  "&CVF(DIFPAN));        
          	       IF CAMNO=2 THEN OUTSTR("  "&CVF(DIFZOM)CRLF) 
					ELSE OUTSTR(" "CRLF);
		     UPDFLG←9;
                       END;
            END;
       END
	ELSE BEGIN
             OUTSTR("CAM_UPDATE: AD NOT AVAILABLE"CRLF);
             UPDFLG←10;
             END;


	IF UPDFLG≠0 THEN BEGIN
	OUTSTR("...TYPE Y TO TRY AGAIN:"CRLF);
        IF   YES 
        THEN BEGIN UPDFLG←0; GOTO ETA; END 
        ELSE BEGIN CAMFLG←UPDFLG;
             IF CAMFLG=10 THEN BEGIN
	     OUTSTR("CAM_UPDATE-FAILED: CAMERA_MODEL NOT UPDATED"CRLF); RETURN;
	     END; END; END;

	IF CAMNO=2 THEN CAMLENS←5;

	PANTIL_CAM (CAMLENS,PANPOT,TILPOT,FOCPOT,ZOOPOT,MCOL,MICOL,LCEN);

        IF CAMNO=2 THEN BEGIN
			FOC[5]←C1+ZOOPOT*C2;
			FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
			FOCLENG[5]←FPOTD[5]/KY;
			L4←P4; L5←P5; L6←P6; L7←P7; L8←P8;
			END;

	⊃ Now to update the global model;

	ARRBLT (CAMERA_MODEL[1,1],MCOL[1,1],9);
  	ARRBLT (CAMERA_MODEL[6,1],MICOL[1,1],9);
	ARRBLT (CAMERA_MODEL[4,1],LCEN[1],3);
	CAMERA_MODEL[5,1] ← PP[CAMLENS,1];
	CAMERA_MODEL[5,2] ← PP[CAMLENS,2];
	CAMERA_MODEL[5,3] ← 1.0;
	CAMERA_MODEL[9,1]←CAMPAN←PPOTD[CAMLENS]*PANPOT+PPOT0[CAMLENS];
        CAMERA_MODEL[9,2]←CAMTIL←TPOTD[CAMLENS]*TILPOT+TPOT0[CAMLENS];
        CAMERA_MODEL[9,3]←CAMRANG← LNS(FOCLEN0[CAMLENS]+
                       		          FOCLENG[CAMLENS]*FOCPOT,CAMLENS);
        CAMERA_MODEL[10,2]←CAMLENS;
	CAMERA_MODEL[10,1]←CAMNO;
	CURCAM[camno] ← GLOBAL NEW (CAMERA_MODEL);

	IF DEB_CAM THEN BEGIN
      		OUTSTR("CAM_UPDATE: "&
 		"CAMPAN="&CVG(CAMPAN)&"  CAMTIL="&CVG(CAMTIL)&"  CAMRANG="&CVG(CAMRANG));
	     	IF CAMNO=2 THEN OUTSTR(CVG(FOC[5])CRLF)
				ELSE OUTSTR(" "CRLF);
    		END;
END "CAM_UPDATE";

SIMPLE PROCEDURE HAT(INTEGER CAMNO);
  BEGIN EXTERNAL PROCEDURE CALLEN;
	EXTERNAL INTEGER LENS;
	IF CAMNO=1 THEN BEGIN
			CALLEN;
			CAMLENS←LENS+1;
			END;

	CAM_UPDATE(CAMNO);
  END "HAT";

SIMPLE REAL PROCEDURE COSQR(REAL A,B,C);
  ⊃ Solves the eq. Acos(x)+Bsin(x)+C=0 for x;
  BEGIN FORTRAN REAL PROCEDURE ACOS(REAL X);
	FORTRAN REAL PROCEDURE SQRT(REAL X);
	REAL K,M,N;
    
    K←A*C; M←A↑2+B↑2; N←B↑2-C↑2;
    IF   B<0
    THEN RETURN(ACOS(-(K/M)+SQRT((K/M)↑2+(N/M))))
    ELSE RETURN(ACOS(-(K/M)-SQRT((K/M)↑2+(N/M))));
  END "COSQR";





SIMPLE MESSAGE PROCEDURE CHNG_LENS(INTEGER RLENS);
  BEGIN EXTERNAL PROCEDURE CALLEN;
	EXTERNAL INTEGER STATUS,LENS;
    IF (RLENS<1)∨(RLENS>4) THEN BEGIN
    OUTSTR("CHNG_LENS-FAILED: LENS NO. OUT OF BOUNDS ("&CVS(RLENS)&")"CRLF); 
    CAMFLG←2; RETURN; END;
    CALLEN;
    IF   LENS≠RLENS-1 
    THEN BEGIN LENS←RLENS-1; STATUS←'20; SERVO; CALLEN;
               IF LENS≠RLENS-1 
	       THEN BEGIN OUTSTR("CHNGE_LENS: I GOOFED, NOW YOU HAVE LENS NO. "&
				 CVS(LENS+1)CRLF); CAMFLG←6; END; END;
    CAMLENS←LENS+1; CAM_UPDATE(1);
  END "CHNG_LENS";

SIMPLE MESSAGE PROCEDURE MOVE_CAM(INTEGER CAMNO;REAL PAN,TILT);
  BEGIN REAL LL2,LL3;
	EXTERNAL INTEGER STATUS,STATE;
	EXTERNAL REAL L2,L3,L4,L5,L6,L7,L8;
	EXTERNAL REAL P4,P5,P6,P7,P8;
	ref← if camno=1 then cref else GROREF[5];
	IF CAMNO=2 THEN CAMLENS←5;	
	LL3←(PAN-PPOT0[CAMLENS])/PPOTD[CAMLENS];
        LL2←(TILT-TPOT0[CAMLENS])/TPOTD[CAMLENS];
        IF CAMNO=1 THEN BEGIN L3←LL3/REF; L2←LL2/REF; END
		ELSE BEGIN
		     L4←LL3/REF; L5←LL2/REF; L6←P6; L7←P7; L8←P8; END;
        IF (LL2≥TLIMIT1[CAMNUM])∨(LL2≤TLIMIT2[CAMNUM]) THEN BEGIN
           OUTSTR("MOVE_CAM-FAILED: TILT OUT OF BOUNDS (LL2="&CVS(LL2)&")"CRLF);
           CAMFLG←4; END;
        IF (LL3≥PLIMIT1[CAMNUM])∨(LL3≤PLIMIT2[CAMNUM]) THEN BEGIN
           OUTSTR("MOVE_CAM-FAILED: PAN OUT OF BOUNDS (LL3="&CVS(LL3)&")"CRLF);
           CAMFLG←3; END;
        IF CAMFLG≠0 THEN RETURN;
        STATUS←'10;
        SERVO;
                          
        CAM_UPDATE(camno);
  END "MOVE_CAM";

SIMPLE MESSAGE PROCEDURE CHNG_FOCUS(INTEGER CAMNO;REAL RANG);
  BEGIN REAL LL1;
	EXTERNAL INTEGER STATUS,STATE;
	EXTERNAL REAL L1,L6;
	EXTERNAL REAL L4,L5,L7,L8,P4,P5,P7,P8;
	ref← if camno=1 then cref else GROREF[5];
	IF CAMNO=2 THEN BEGIN
		CAMLENS←5;
		FOCLENG[5]←FPOTD[5]/KY;
		FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
		L4←P4; L5←P5; L7←P7; L8←P8;
		END;			
       LL1←(LNS(RANG,CAMLENS)-FOCLEN0[CAMLENS])/FOCLENG[CAMLENS];
       IF (LL1≥FLIMIT1[CAMNUM])∨(LL1≤FLIMIT2[CAMNUM]) 
       THEN BEGIN OUTSTR("CHNGE_FOC-FAILED: FOCUS OUT OF BOUNDS (LL1="&
			  CVS(LL1)&")"CRLF); CAMFLG←5; RETURN; END;
       IF CAMNO=1 THEN L1←LL1/REF  ELSE L6←LL1/REF; STATUS←'10; SERVO; 
       CAM_UPDATE(camno);
  END "CHNG_FOCUS";

  SIMPLE MESSAGE PROCEDURE CHNG_ZOOM(REAL FOC_LGTH);
	BEGIN "CHNG_ZOOM"
	EXTERNAL REAL L7;
	EXTERNAL REAL L4,L5,L6,L8,P4,P5,P6,P8;
	L4←P4; L5←P5; L6←P6; L8←P8;
	L7←(FOC_LGTH-C1)/(C2*REF);
	SERVO;
	CAM_UPDATE(2);
	END "CHNG_ZOOM";

SIMPLE MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC);
  BEGIN REAL XCC,YCC,ZCC,D,RANG,PAN,TILT,LL1,LL2,LL3;
	EXTERNAL PROCEDURE CALLEN;
	EXTERNAL INTEGER LENS,STATUS;
	EXTERNAL REAL L1,L2,L3,L4,L5,L6,L7,L8;
	FORTRAN REAL PROCEDURE SIN(REAL X);
	FORTRAN REAL PROCEDURE COS(REAL X);

    ref← if camno=1 then cref else sref;
    IF CAMNO=1 THEN IF (RLENS<1)∨(RLENS>4) THEN BEGIN
    OUTSTR("CAM_CENTER-FAILED: LENS NO. OUT OF BOUNDS ("&CVS(RLENS)&")"CRLF); 
    CAMFLG←2; RETURN; END;
    IF CAMNO=2 THEN IF RLENS≠5 THEN
	BEGIN
	OUTSTR("WRONG LENS NUMBER FOR SIERRA"&CVS(RLENS)CRLF);
	RETURN;
	END;

    XCC←P0[RLENS,1]-XTC; YCC←P0[RLENS,2]-YTC; ZCC←P0[RLENS,3]-ZTC;
    PAN←COSQR(YCC,-XCC,DP[RLENS,1]);
	IF (CAMNO=2) ∧ (P0[RLENS,2]+DP[RLENS,1]-YTC<0) THEN PAN←-PAN;

    D←XCC*COS(PAN)+YCC*SIN(PAN);
    TILT←COSQR(ZCC,-D,-DP[RLENS,2]);
    RANG←D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];

    LL3←(PAN-PPOT0[RLENS])/PPOTD[RLENS];
    LL2←(TILT-TPOT0[RLENS])/TPOTD[RLENS];
    LL1←(LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
    IF (LL2≥TLIMIT1[CAMNO])∨(LL2≤TLIMIT2[CAMNO]) THEN BEGIN
       OUTSTR("CAM_CENTER¬FAILED: TILT OUT OF BOUNDS (LL2="&CVE(LL2)&")"CRLF);
       CAMFLG←4; END;
    IF (LL3≥PLIMIT1[CAMNO])∨(LL3≤PLIMIT2[CAMNO]) THEN BEGIN
       OUTSTR("CAM_CENTER-FAILED: PAN OUT OF BOUNDS (LL3="&CVE(LL3)&")"CRLF);
       CAMFLG←3; END;
    IF (LL1≥FLIMIT1[CAMNO])∨(LL1≤FLIMIT2[CAMNO]) THEN BEGIN
       OUTSTR("CAM_CENTER-FAILED: FOCUS OUT OF BOUNDS (LL1="&CVE(LL1)&")"CRLF);
       CAMFLG←5; END;
    IF CAMFLG≠0 THEN RETURN;

    IF CAMNO=1 THEN GROREF[RLENS]←CREF;
    L1←L6←LL1/GROREF[RLENS]; L2←L5←LL2/GROREF[RLENS]; L3←L4←LL3/GROREF[RLENS];

    IF CAMNO=1 THEN
	    BEGIN
	    CALLEN;
	    IF   LENS≠RLENS-1 THEN BEGIN LENS←RLENS-1; STATUS←'30; END ELSE STATUS←'10;
	    SERVO;
	    CALLEN;
	    IF   LENS≠RLENS-1 
	    THEN BEGIN OUTSTR("CAM_CENTER: I GOOFED, NOW YOU HAVE LENS NO. "&
	                       CVS(LENS+1)CRLF); CAMFLG←6; END;
	    CAMLENS←LENS+1;
	    END
	   ELSE  SERVO;
    CAM_UPDATE(camno);

  END "CAM_CENTER";



SIMPLE MESSAGE PROCEDURE CAM_PRED(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC);
  BEGIN REAL XCC,YCC,ZCC,D,RANG,PAN,TILT,LL1,LL2,LL3;
	FORTRAN REAL PROCEDURE SIN(REAL X);
	FORTRAN REAL PROCEDURE COS(REAL X);

    IF CAMNO=1 THEN
	    IF (RLENS<1)∨(RLENS>4) THEN BEGIN
	    OUTSTR("CAM_PRED-FAILED: LENS NO. OUT OF BOUNDS ("&CVS(RLENS)&")"CRLF); 
	    CAMFLG←2; RETURN; END
		ELSE IF RLENS≠5 THEN
			BEGIN
			OUTSTR("CAM_PRED-FAILED: LENS NO. OUT OF BOUNDS"&CVS(RLENS)CRLF);
			RETURN;
			END;

    XCC←P0[RLENS,1]-XTC; YCC←P0[RLENS,2]-YTC; ZCC←P0[RLENS,3]-ZTC;
    PAN←COSQR(YCC,-XCC,DP[RLENS,1]);
    D←XCC*COS(PAN)+YCC*SIN(PAN);
    TILT←COSQR(ZCC,-D,-DP[RLENS,2]);
    RANG←D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];

    LL3←(PAN-PPOT0[RLENS])/PPOTD[RLENS];
    LL2←(TILT-TPOT0[RLENS])/TPOTD[RLENS];
    LL1←(LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
    IF (LL2≥TLIMIT1[RLENS])∨(LL2≤TLIMIT2[RLENS]) THEN BEGIN
       OUTSTR("CAM_PRED¬FAILED: TILT OUT OF BOUNDS (LL2="&CVS(LL2)&")"CRLF);
       CAMFLG←4; END;
    IF (LL3≥PLIMIT1[RLENS])∨(LL3≤PLIMIT2[RLENS]) THEN BEGIN
       OUTSTR("CAM_PRED-FAILED: PAN OUT OF BOUNDS (LL3="&CVS(LL3)&")"CRLF);
       CAMFLG←3; END;
    IF (LL1≥FLIMIT1[RLENS])∨(LL1≤FLIMIT2[RLENS]) THEN BEGIN
       OUTSTR("CAM_PRED-FAILED: FOCUS OUT OF BOUNDS (LL1="&CVS(LL1)&")"CRLF);
       CAMFLG←5; END;
    IF CAMFLG≠0 THEN RETURN;

    PANTIL_CAM (RLENS,LL3,LL2,LL1,ZOOPOT,MCOL,MICOL,LCEN);
		⊃ temporary hack;
	ARRBLT (CAMERA_PREDI[1,1],MCOL[1,1],9);
  	ARRBLT (CAMERA_PREDI[6,1],MICOL[1,1],9);
	ARRBLT (CAMERA_PREDI[4,1],LCEN[1],3);
	CAMERA_PREDI[5,1] ← PP[CAMLENS,1];
	CAMERA_PREDI[5,2] ← PP[CAMLENS,2];
	CAMERA_PREDI[5,3] ← 1.0;
	CAMERA_PREDI[9,1]←PAN;
        CAMERA_PREDI[9,2]←TILT;
        CAMERA_PREDI[9,3]←RANG;
	camera_predi[10,1]←camno;
        CAMERA_PREDI[10,2]←RLENS;

  END "CAM_PRED";

SIMPLE MESSAGE PROCEDURE CAM_INIT(INTEGER CAMNO);
  BEGIN	CAMFLG←0;
	IF CAMNO=1 THEN
	        FOR BLOCK←1 STEP 1 UNTIL 4 DO DATXFR;	⊃ JUST READ IN A MODEL;
	IF CAMNO=2 THEN 
		BEGIN 
		BLOCK←1;
		DATXFR;
		END;
 END"CAM_INIT";
SIMPLE PROCEDURE TESTIT;
  BEGIN STRING COORD;
        REAL X,Y,Z,NPAN,NTILT,NRANG;
        INTEGER NLENS,REQUEST;
	STRING ST;
        LABEL TIT,TES;
TIT:	OUTSTR("CAMERA NO.?"CRLF);
	IF ST←INCHWL="1" THEN CAMNUM←1
		ELSE CAMNUM←2;

       	OUTSTR("1-CHANGE THE LENS"CRLF&
	       "2-PAN AND TILT THE CAMERA"CRLF&
	       "3-FOCUS ON A GIVEN RANGE"CRLF&
	       "4-CENTER THE CAMERA ON A GIVEN POINT"CRLF&
	       "5-COMPLEMENT DEB_CAM"CRLF& 
	       "6-CHANGE THE ZOOM"CRLF);
   TES: OUTSTR("...TYPE THE NO. OF THE PROCEDUE YOU WANT TO TEST NOW="CRLF);
	REQUEST←CVD(INCHWL); 
	IF (REQUEST<1)∨(REQUEST>6) THEN BEGIN 
	OUTSTR("TESTIT-FAILED: ILLEGAL PROCEDURE NO ("&CVS(REQUEST)&")"CRLF);
        GOTO TIT; END;
	CASE REQUEST OF BEGIN
	BEGIN ⊃ 0;						END;
	BEGIN ⊃ 1; OUTSTR("...TYPE NO. OF LENS="CRLF);
		   NLENS←CVD(INCHWL); CHNG_LENS(NLENS);	END;
	BEGIN ⊃ 2; OUTSTR("...TYPE PAN AND TILT IN RADS,"&
			  " EACH FOLLOWED BY C.R="CRLF);
		   COORD←INCHWL; NPAN←REALSCAN(COORD,MESS);
		   COORD←INCHWL; NTILT←REALSCAN(COORD,MESS);
 		   MOVE_CAM(CAMNUM,NPAN,NTILT);			END;
	BEGIN ⊃ 3; OUTSTR("...TYPE RANGE TO FOCUS ON="CRLF);
		   COORD←INCHWL; NRANG←REALSCAN(COORD,MESS);
		   CHNG_FOCUS(CAMNUM,NRANG);				END;
	BEGIN ⊃ 4; OUTSTR("TYPE IN LENS N0."&
			  "AND X,Y,Z OF CENTER IN TABLE COORDINATES"CRLF&
                          "EACH FOLLOWED BY A C.R.="CRLF);
		   COORD←INCHWL; NLENS←CVD(COORD);
        	   COORD←INCHWL; X←REALSCAN(COORD,MESS);
		   COORD←INCHWL; Y←REALSCAN(COORD,MESS);
		   COORD←INCHWL; Z←REALSCAN(COORD,MESS);
		   CAM_CENTER(NLENS,CAMNUM,X,Y,Z);		END;
	BEGIN ⊃ 5; DEB_CAM←¬DEB_CAM; GOTO TES;			END;

	BEGIN ⊃ 6; OUTSTR("DESIRED FOCAL LENGTH"CRLF);
		   COORD←INCHWL; FOC_LGTH←REALSCAN(COORD,MESS);
				 CHNG_ZOOM(FOC_LGTH);		END;
							


 	END;

	OUTSTR("...TYPE Y TO  TEST AGAIN:"CRLF);
	IF INCHWL="Y" THEN BEGIN CAMFLG←0; GOTO TIT; END;
  END "TESTIT";
⊃ HERE BEGINS EXECUTION ;
	CAMFLG←0; TYP_CAM←TRUE; DEB_CAM←FALSE; YES_CAM←TRUE;
	FOR CAMNUM←1 STEP 1 UNTIL 2 DO
		BEGIN
	 	CAM_INIT(CAMNUM);
	        HAT(CAMNUM);
		END;
	IF   RUN=0
 	THEN BEGIN OUTSTR("...TYPE Y FOR TEST MODE:"CRLF);
	           IF YES THEN TESTIT; END
	ELSE BEGIN
 	  PUT_DATA(0,0,"CAM");        ⊃ DECLARE YOUR NAME ;
	  OUTSTR("CAM-ACTIVATED"CRLF);
 	  WHILE TRUE DO BEGIN 
	    MESS ← GET_ENTRY ('120,NULL,"CAM",NULL);
	    CAMFLG←0;
	    MESS ← QUEUE ('600,MESS); ⊃ ACTIVATE AND ACKNOWLEDGE ;
 	  END; END;
END "GILCAM";